/*
 * Copyright (c) 2021 Nordic Semiconductor ASA
 *
 * SPDX-License-Identifier: Apache-2.0
 */
#include <zephyr/shell/shell.h>
#include <zephyr/shell/shell_uart.h>
#include <zephyr/drivers/uart.h>
#include <zephyr/device.h>
#include <ctype.h>
#include <stdio.h>
#include <stdlib.h>
#include <zephyr/drivers/flash.h>
#include <zephyr/sys/reboot.h>
#include <zephyr/task_wdt/task_wdt.h>
#include <zephyr/drivers/watchdog.h>
#include <../../dolphin_common/include/motor.h>
#include <../../dolphin_common/include/relay.h>
#include <../../dolphin_common/include/common.h>
#include <../../dolphin_common/include/servo.h>
#include <../../dolphin_common/include/stepmotor.h>
#include <../../dolphin_common/include/jetson_nano.h>
LOG_MODULE_REGISTER(shell);
/*********************************************************************
 * 需要的标志定义
 *********************************************************************/
#define APP_OFFSET_ADDR       	0x00020000
#define STM32_FLASH_BASE 		0x08000000
#define APP_SIZE_SIGN_OFFSET_ADDR 0x000EFFF0  //flash的一个区域划为app程序大小标记
#define DOWNLOAD_IDLE 0
#define DOWNLOAD_BOOT 1
#define DOWNLOAD_APP  2
#define DOWNLOAD_DEST 3
#define CHAR_1 0x18
#define CHAR_2 0x11

/*********************************************************************
 * 全局变量与外部变量声明
 *********************************************************************/
extern k_tid_t download_tid;
extern k_tid_t nano_tx_tid;
extern uint8_t download_type;
extern uint32_t DOWN_DEST_ADDR;
extern uint32_t FLASH_BASE_ADDR;
extern const struct device *uart_motor;
extern const struct device *uart_battery ;
extern const struct device *uart_imu0 ;
extern const struct device *uart_avoid ;
extern const struct device *const hw_wdt_dev ;
#define	 SERVO_HEAD_X 			2
#define	 SERVO_HEAD_Y 			3
#define	 SERVO_FIN_LEFT_OUT 	4
#define	 SERVO_FIN_LEFT_IN 		0
#define	 SERVO_FIN_RIGHT_IN  	1
#define	 SERVO_FIN_RIGHT_OUT 	5

/*********************************************************************
 * 外部函数声明
 *********************************************************************/
void sys_reboot	(int type);
void download_function(void *p1, void *p2, void *p3);
void int2str(int n, char *str);
int str2int(const char *str);
void jump_to_addr(uint32_t addr);

/*********************************************************************
 * shell旁路开通任务
 *********************************************************************/
int set_bypass(const struct shell *sh, shell_bypass_cb_t bypass)
{
	static bool in_use;

	if (bypass && in_use) {
		shell_error(sh, "Sample supports setting bypass on single instance.");

		return -EBUSY;
	}

	in_use = !in_use;
	if (in_use) {
		shell_print(sh, "Bypass started, press ctrl-x ctrl-q to escape");
		in_use = true;
	}

	shell_set_bypass(sh, bypass);

	return 0;
}


void bypass_cb(const struct shell *sh, uint8_t *data, size_t len)
{
	static uint8_t tail;
	bool escape = false;

	/* Check if escape criteria is met. */
	if (tail == CHAR_1 && data[0] == CHAR_2) {
		escape = true;
	} else {
		for (int i = 0; i < (len - 1); i++) {
			if (data[i] == CHAR_1 && data[i + 1] == CHAR_2) {
				escape = true;
				break;
			}
		}
	}

	if (escape) {
		shell_print(sh, "Exit bypass");
		set_bypass(sh, NULL);
		tail = 0;
		return;
	}

	/* Store last byte for escape sequence detection */
	tail = data[len - 1];

	/* Do the data processing. */
	for (int i = 0; i < len; i++) {
		shell_fprintf(sh, SHELL_INFO, "%02x ", data[i]);
	}
	shell_fprintf(sh, SHELL_INFO, "| ");

	for (int i = 0; i < len; i++) {
		shell_fprintf(sh, SHELL_INFO, "%c", data[i]);
	}
	shell_fprintf(sh, SHELL_INFO, "\n");

}

static int cmd_bypass(const struct shell *sh, size_t argc, char **argv)
{
	return set_bypass(sh, bypass_cb);
}

SHELL_CMD_ARG_REGISTER(bypass, NULL, "Bypass shell", cmd_bypass, 1, 0);



/*********************************************************************
 * 上位机通讯任务
 *********************************************************************/

void shell_uninit_nano_cb(const struct shell *shell, int res)
{
	const struct device *const dev = DEVICE_DT_GET(DT_CHOSEN(zephyr_shell_uart));
		/* connect uart to my handler */
	uart_irq_callback_user_data_set(dev, nano_uart_callback, NULL);
	uart_irq_rx_enable(dev);
	k_sleep(K_MSEC(5));
}

static int cmd_nano_ctr(const struct shell *sh, size_t argc, char **argv)
{
	shell_uninit(shell_backend_uart_get_ptr(), shell_uninit_nano_cb); 
	shell_print(sh, "nano control");
	k_thread_resume(nano_tx_tid);
	k_sleep(K_MSEC(50));
	return 0;
}
SHELL_CMD_REGISTER(nano_ctr, NULL, "nano control shell", cmd_nano_ctr);
/*********************************************************************
 * shell下载任务
 *********************************************************************/
void shell_uninit_download_cb(const struct shell *shell, int res)
{
	__ASSERT_NO_MSG(res >= 0);
	const struct device *const dev = DEVICE_DT_GET(DT_CHOSEN(zephyr_shell_uart));
	uart_irq_tx_disable(dev);
	uart_irq_rx_disable(dev);
}

static int cmd_download_dest(const struct shell *shell, size_t argc, char **argv)
{
	uint32_t dest = strtoul(argv[1],NULL,16) & 0xffffffff;
	uint8_t buf[10];
	static const struct device *flash_device =
	DEVICE_DT_GET_OR_NULL(DT_CHOSEN(zephyr_flash_controller));
	flash_read(flash_device,APP_SIZE_SIGN_OFFSET_ADDR,buf ,10);
	int32_t app_size  = str2int(buf) ;

	if(dest  < FLASH_BASE_ADDR || dest % 4 || (dest > FLASH_BASE_ADDR && dest < FLASH_BASE_ADDR + app_size)){
		shell_print(shell, "dest error,please check\r\n");
		return -1;
	}

	DOWN_DEST_ADDR = dest;
	download_type = DOWNLOAD_DEST;
	k_thread_start(download_tid);
	k_thread_resume(download_tid);
	shell_print(shell, "download to dest 0x%x",dest);
	
	return 0;
}

SHELL_STATIC_SUBCMD_SET_CREATE(sub_download,
	SHELL_CMD_ARG(dest, NULL, "download to destination ", cmd_download_dest,2,0),
	SHELL_SUBCMD_SET_END
);
SHELL_CMD_REGISTER(download, &sub_download, "download", NULL);

/*********************************************************************
 * shell 跳转bootloader任务
 *********************************************************************/
static int cmd_go_jump_boot(const struct shell *shell, size_t argc, char **argv)
{
	ARG_UNUSED(argc);
	ARG_UNUSED(argv);

	k_sleep(K_MSEC(10));

  	Relay_close(ALL);
    k_sleep(K_MSEC(500));
	
	shell_print(shell, "go jump to bootloader");
	uart_irq_tx_disable(uart_motor);
	uart_irq_rx_disable(uart_motor);
	uart_irq_tx_disable(uart_imu0);
	uart_irq_rx_disable(uart_imu0);

	wdt_disable(hw_wdt_dev);
	jump_to_addr(STM32_FLASH_BASE);

	return 0;
}

SHELL_CMD_REGISTER(goboot, NULL, "jump to boot", cmd_go_jump_boot);

/*********************************************************************
 * shell reinit和uninit
 *********************************************************************/
void shell_reinit_trigger(void)
{
	const struct device *const dev = DEVICE_DT_GET(DT_CHOSEN(zephyr_shell_uart));

	bool log_backend = CONFIG_SHELL_BACKEND_SERIAL_LOG_LEVEL > 0;
	uint32_t level =
		(CONFIG_SHELL_BACKEND_SERIAL_LOG_LEVEL > LOG_LEVEL_DBG) ?
		CONFIG_LOG_MAX_LEVEL : CONFIG_SHELL_BACKEND_SERIAL_LOG_LEVEL;

	shell_init(shell_backend_uart_get_ptr(), dev,
		shell_backend_uart_get_ptr()->ctx->cfg.flags,
		log_backend, level);
}

static void direct_uart_callback(const struct device *dev, void *user_data)
{
	static uint8_t buf[1];
	static bool tx_busy;

	uart_irq_update(dev);

	if (uart_irq_rx_ready(dev)) {
		while (uart_fifo_read(dev, buf, sizeof(buf))) {
			if (!tx_busy) {
				uart_irq_tx_enable(dev);
			}
		}
	}

	if (uart_irq_tx_ready(dev)) {
		if (!tx_busy) {
			(void)uart_fifo_fill(dev, buf, sizeof(buf));
			tx_busy = true;
		} else {
			tx_busy = false;
			uart_irq_tx_disable(dev);
			if (buf[0] == 27) { //按下Esc停止跳转与开启shell
				uart_irq_rx_disable(dev);
				shell_reinit_trigger();
			}
		}
	}

}

void shell_uninit_cb(const struct shell *shell, int res)
{
	__ASSERT_NO_MSG(res >= 0);
	const struct device *const dev = DEVICE_DT_GET(DT_CHOSEN(zephyr_shell_uart));
		/* connect uart to my handler */
	uart_irq_callback_user_data_set(dev, direct_uart_callback, NULL);
	uart_irq_rx_enable(dev);

}

/*********************************************************************
 * shell重启任务
 *********************************************************************/
static int cmd_reboot(const struct shell *shell, size_t argc, char **argv)
{
	ARG_UNUSED(argc);
	ARG_UNUSED(argv);
    k_sleep(K_MSEC(10));
	uart_irq_tx_disable(uart_motor);
	uart_irq_rx_disable(uart_motor);
	uart_irq_tx_disable(uart_imu0);
	uart_irq_rx_disable(uart_imu0);
	wdt_disable(hw_wdt_dev);
	sys_reboot(SYS_REBOOT_COLD);
	return 0;
}

SHELL_CMD_REGISTER(reboot, NULL, "reboot", cmd_reboot);

/*********************************************************************
 * shell 电机控制任务
 *********************************************************************/
static int cmd_motor_speed_ctr(const struct shell *shell, size_t argc, char **argv)
{
	if(argc < 2){
			shell_print(shell, "please input the speed value\n");
			return -1;
		}
	uint32_t spd = strtoul(argv[1],NULL,10);
	Motor_Ctr_API(uart_motor,Speed_ClosedLoop_ctr(spd));
	shell_print(shell, "motor speed control  success\n");
	return 0;
}
static int cmd_motor_torque_ctr(const struct shell *shell, size_t argc, char **argv)
{
	if(argc < 2){
			shell_print(shell, "please input the torque value\n");
			return -1;
		}
	uint32_t torque = strtoul(argv[1],NULL,10);
	Motor_Ctr_API(uart_motor,Torque_ClosedLoop_ctr(torque));
	shell_print(shell, "motor torque control success\n");
	return 0;
}
static int cmd_pos_track_ctr(const struct shell *shell, size_t argc, char **argv)
{
	if(argc < 2){
			shell_print(shell, "please input the pos_track value\n");
			return -1;
		}
	int32_t pos = strtoul(argv[1],NULL,10);
	Motor_Ctr_API(uart_motor,Pos_Track_closedLoop_ctr(pos));
	shell_print(shell, "motor pos track control success\n");
	return 0;
}
static int cmd_pos_inc_ctr(const struct shell *shell, size_t argc, char **argv)
{
	if(argc < 2){
			shell_print(shell, "please input the pos_inc value\n");
			return -1;
	}
	if(argc < 3){
			shell_print(shell, "please input the maxSpeed\n");
			return -1;
	}
	int32_t pos = strtoul(argv[1],NULL,10);
	uint32_t maxSpeed = (uint16_t)strtoul(argv[2],NULL,10);
	Motor_Ctr_API(uart_motor,Pos_Inc_closedLoop_ctr(pos,maxSpeed));
	shell_print(shell, "motor pos increment control success\n");
	return 0;
}	
static int cmd_motor_status_ctr(const struct shell *shell, size_t argc, char **argv)
{
	Motor_Ctr_API(uart_motor,Read_Status_one());
	k_sleep(K_MSEC(100));
	Motor_Ctr_API(uart_motor,Read_Status_two());
	LOG_INF("motor temperature: %d °c" "  motor speed: "LOG_FLOAT_MARKER "dps""  motor volt: " LOG_FLOAT_MARKER "V""  motor curr: " LOG_FLOAT_MARKER "A""  motor degree: " LOG_FLOAT_MARKER "°"" \r\n",Motor_Para_g.motor_tem, LOG_FLOAT(Motor_Para_g.motor_spd), LOG_FLOAT(Motor_Para_g.motor_vol),LOG_FLOAT(Motor_Para_g.motor_curr), LOG_FLOAT(Motor_Para_g.motor_degree));
	shell_print(shell, "motor status get success\n");
	return 0;
}
static int cmd_motor_stop_ctr(const struct shell *shell, size_t argc, char **argv)
{	
	Motor_Ctr_API(uart_motor,Motor_Stop());
	shell_print(shell, "motor stop success\n");
	return 0;
}
static int cmd_motor_shutdown_ctr(const struct shell *shell, size_t argc, char **argv)
{
	Motor_Ctr_API(uart_motor,Motor_Shutdown());
	shell_print(shell, "motor shutdown success\n");
	return 0;
}
static int cmd_motor_write_acc_ctr(const struct shell *shell, size_t argc, char **argv)
{
	if(argc < 2){
			shell_print(shell, "please input the acc value\n");
			return -1;
		}
	uint32_t acc = strtoul(argv[1],NULL,10);
	Motor_Ctr_API(uart_motor,Write_Acc(acc));
	shell_print(shell, "motor write acc success\n");	
	return 0;
}
static int cmd_motor_read_acc_ctr(const struct shell *shell, size_t argc, char **argv)
{
	Motor_Ctr_API(uart_motor,Read_Acc());
	shell_print(shell, "motor read acc success\n");
	return 0;
}
static int cmd_motor_Motor_Reset_system(const struct shell *shell, size_t argc, char **argv)
{
	Motor_Ctr_API(uart_motor,Motor_Reset_system());
	shell_print(shell, "Motor Reset system success\n");
	return 0;
}
SHELL_STATIC_SUBCMD_SET_CREATE(sub_motor_ctr,
	SHELL_CMD(speed, NULL, "motor speed control ", cmd_motor_speed_ctr),
	SHELL_CMD(torque, NULL, "motor torque control ", cmd_motor_torque_ctr),
	SHELL_CMD(pos_track, NULL, "motor pos_track control ", cmd_pos_track_ctr),
	SHELL_CMD(pos_inc, NULL, "motor pos_inc control ", cmd_pos_inc_ctr),
	SHELL_CMD(status, NULL, "motor status control ", cmd_motor_status_ctr),
	SHELL_CMD(stop, NULL, "motor stop control ", cmd_motor_stop_ctr),
	SHELL_CMD(shutdown, NULL, "motor shutdown control ", cmd_motor_shutdown_ctr),
	SHELL_CMD(write_acc, NULL, "motor write_acc control ", cmd_motor_write_acc_ctr),
	SHELL_CMD(read_acc, NULL, "motor read_acc control ", cmd_motor_read_acc_ctr),
	SHELL_CMD(motor_Reset, NULL, "motor_Reset_system ", cmd_motor_Motor_Reset_system),
	SHELL_SUBCMD_SET_END
);
SHELL_CMD_REGISTER(motor_ctr, &sub_motor_ctr, "motor control", NULL);


/*********************************************************************
 * shell 舵机控制任务
 *********************************************************************/
static int cmd_single_servo_ctr(const struct shell *shell, size_t argc, char **argv)
{
	k_sleep(K_MSEC(10));
	if(strcmp(argv[1],"head_x") == 0){
		if(argc < 3){
			shell_print(shell, "please input the angle\n");
			return -1;
		}
		float angle = my_atof(argv[2]);
		Servo_ctr_single(SERVO_HEAD_X,angle);
		shell_print(shell, "servo control  success\n");
	}
	else if(strcmp(argv[1],"head_y") == 0){
		if(argc < 3){
			shell_print(shell, "please input the angle\n");
			return -1;
		}
		float angle = my_atof(argv[2]);
		Servo_ctr_single(SERVO_HEAD_Y,angle);
		shell_print(shell, "servo control  success\n");
	}
	else if(strcmp(argv[1],"fin_left_out") == 0){
		if(argc < 3){
			shell_print(shell, "please input the angle\n");
			return -1;
		}
		float angle = my_atof(argv[2]);
		Servo_ctr_single(SERVO_FIN_LEFT_OUT,angle);
		shell_print(shell, "servo control  success\n");
	}
	else if(strcmp(argv[1],"fin_left_in") == 0){
		if(argc < 3){
			shell_print(shell, "please input the angle\n");
			return -1;
		}
		float angle = my_atof(argv[2]);
		Servo_ctr_single(SERVO_FIN_LEFT_IN,angle);
		shell_print(shell, "servo control  success\n");
	} 
	else if(strcmp(argv[1],"fin_right_in") == 0){
		if(argc < 3){
			shell_print(shell, "please input the angle\n");
			return -1;
		}
		float angle = my_atof(argv[2]);
		Servo_ctr_single(SERVO_FIN_RIGHT_IN,angle);
		shell_print(shell, "servo control  success\n");
	}
	else if(strcmp(argv[1],"fin_right_out") == 0){
		if(argc < 3){
			shell_print(shell, "please input the angle\n");
			return -1;
		}
		float angle = my_atof(argv[2]);
		Servo_ctr_single(SERVO_FIN_RIGHT_OUT,angle);
		shell_print(shell, "servo control  success\n");
	}
	else{
		shell_print(shell, "check you input\n");
	}

	return 0;
}
static int cmd_all_servo_ctr(const struct shell *shell, size_t argc, char **argv)
{
	k_sleep(K_MSEC(10));
	if(argc < 7){
			shell_print(shell, "please input the angle\n");
			return -1;
		}
	Servo_Ctr servo_ctr_angle_val;
	servo_ctr_angle_val.servo_head_x = my_atof(argv[1]);
	servo_ctr_angle_val.servo_head_y = my_atof(argv[2]);
	servo_ctr_angle_val.servo_fin_left_out = my_atof(argv[3]);
	servo_ctr_angle_val.servo_fin_left_in = my_atof(argv[4]);
	servo_ctr_angle_val.servo_fin_right_in = my_atof(argv[5]);
	servo_ctr_angle_val.servo_fin_right_out = my_atof(argv[6]);
	Servo_ctr_API(servo_ctr_angle_val);
	shell_print(shell, "servo control  success\n");
	return 0;
}
SHELL_STATIC_SUBCMD_SET_CREATE(sub_servo_ctr,
	SHELL_CMD(single, NULL, "single servo control ", cmd_single_servo_ctr),
	SHELL_CMD(all, NULL, "all servo control", cmd_all_servo_ctr),
	SHELL_SUBCMD_SET_END
);
SHELL_CMD_REGISTER(servo_ctr, &sub_servo_ctr, "servo control", NULL);

/*********************************************************************
 * shell 步进电机控制任务
 *********************************************************************/
static int cmd_stepmotor_ctr(const struct shell *shell, size_t argc, char **argv)
{
	k_sleep(K_MSEC(10));
	Stepmotor_Ctr  Stepmotor_Ctr_val;
	Stepmotor_Ctr_val.dist = my_atof(argv[1]);
	Stepmotor_ctr_API(Stepmotor_Ctr_val);
	shell_print(shell, "stepmotor control  success ! dist is %d\n",(int)Stepmotor_Ctr_val.dist);
	return 0;
}
SHELL_CMD_ARG_REGISTER(stepmotor_ctr, NULL, "step motor control", cmd_stepmotor_ctr,2,0);

/*********************************************************************
 * shell 继电器控制任务
 *********************************************************************/
static int cmd_relay_on_ctr(const struct shell *shell, size_t argc, char **argv)
{
	k_sleep(K_MSEC(10));
	if(argc < 2){
			shell_print(shell, "please input the relay type\n");
			return -1;
		}
	
	if(strcmp(argv[1],"motor") == 0){
		Relay_open(MOTOR_RELAY);
		shell_print(shell, "relay control  success\n");
	}
	else if(strcmp(argv[1],"stepmotor") == 0){
		Relay_open(STEPMOTOR_RELAY);
		shell_print(shell, "relay control  success\n");
	}
	else if(strcmp(argv[1],"servo") == 0){
		Relay_open(SERVO_RELAY);
		shell_print(shell, "relay control  success\n");
	}
	else if(strcmp(argv[1],"all") == 0){
		Relay_open(ALL);
		shell_print(shell, "relay control  success\n");
	}
	else{
		shell_print(shell, "check you input\n");
	}
	return 0;
}
static int cmd_relay_off_ctr(const struct shell *shell, size_t argc, char **argv)
{
	k_sleep(K_MSEC(10));
	if(argc < 2){
			shell_print(shell, "please input the angle\n");
			return -1;
		}
	
	if(strcmp(argv[1],"motor") == 0){
		Relay_close(MOTOR_RELAY);
		shell_print(shell, "relay control  success\n");
	}
	else if(strcmp(argv[1],"stepmotor") == 0){
		Relay_close(STEPMOTOR_RELAY);
		shell_print(shell, "relay control  success\n");
	}
	else if(strcmp(argv[1],"servo") == 0){
		Relay_close(SERVO_RELAY);
		shell_print(shell, "relay control  success\n");
	}
	else if(strcmp(argv[1],"all") == 0){
		Relay_close(STEPMOTOR_RELAY);
		Relay_close(STEPMOTOR_RELAY);
		Relay_close(SERVO_RELAY);
		shell_print(shell, "relay control  success\n");
	}
	else{
		shell_print(shell, "check you input\n");
	}
	return 0;
}
SHELL_STATIC_SUBCMD_SET_CREATE(sub_relay_ctr,
	SHELL_CMD(on, NULL, "switch on relay control ", cmd_relay_on_ctr),
	SHELL_CMD(off, NULL, "switch off relay control", cmd_relay_off_ctr),
	SHELL_SUBCMD_SET_END
);
SHELL_CMD_REGISTER(relay_ctr, &sub_relay_ctr, "relay control", NULL);